COMMENT ⊗   VALID 00002 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	! CREATED DEC 30, 1977  ALL PNT,MSM CHANGED TO PNT,HE 
C00006 ENDMK
C⊗;
! CREATED DEC 30, 1977  ALL PNT,MSM CHANGED TO PNT,HE ;

REQUIRE "TLKF5A[PNT,HE]" LOAD_MODULE;
	! NOTE THAT TLKEF5[PNT,MSM] IS NOT THE SAME AS TLKEF5[11,BES];

REQUIRE "MOVARM[PNT,HE]" LOAD_MODULE;


EXTERNAL INTEGER PROCEDURE TLKEF5(REAL ARRAY TNS,ANGLES);
	! fills TNS with transpose(<arm trans>), ANGLES with
	  joint angles (ANGLES[7]=hand) in degrees & inches.
	  Returns 0 if all ok, otherwise returns 1.

	MAY 17, 1977 - SEEMS TO FILL TNS WITH 3*4 MATRIX;
	;

PROCEDURE READ_TRANS(STRING COLOR; REAL ARRAY A);
	BEGIN
	INTEGER I,J;
	OWN REAL ARRAY BESTNS[1:4,1:3], BESANGLES[1:7];
	IF EQU(COLOR,"BLUE")
	THEN BEGIN
		IF TLKEF5(BESTNS,BESANGLES) THEN 
			ABORT1("ERROR IN READING ARM",NULL);
		ARRCLR(A);
		FOR I← 1 STEP 1 UNTIL 3
			DO FOR J←1 STEP 1 UNTIL 4
				DO A[I,J]←BESTNS[J,I];
		A[4,4]←1.0;

	        BHAND←BESANGLES[7];	! RETURNS BLUE HAND OPENING;
	     END
	ELSE ABORT1("CANT READ YELLOW ARM YET",NULL);

	END;

PROCEDURE READ_BLUE(REAL ARRAY A);
	READ_TRANS("BLUE", A);

! PROCEDURE READ_YELLOW(REAL ARRAY A);
!	READ_TRANS("YELLOW", A);

EXTERNAL PROCEDURE DR_B_D(INTEGER J; REAL DIF,TT(2.0));
EXTERNAL PROCEDURE DR_B_A(INTEGER J; REAL NEWJ,TT(2.0));
EXTERNAL PROCEDURE DR_Y_D(INTEGER J; REAL DIF,TT(2.0));
EXTERNAL PROCEDURE DR_Y_A(INTEGER J; REAL NEWJ,TT(2.0));
EXTERNAL PROCEDURE MOVE_B(REAL ARRAY A;INTEGER PARK(0);real tt(2.0));
EXTERNAL PROCEDURE MOVE_Y(REAL ARRAY A;INTEGER PARK(0); real tt(2.0));
EXTERNAL PROCEDURE OPNB_D(REAL DIF,TT(2.0));
EXTERNAL PROCEDURE OPNB_A(REAL NEWH,TT(2.0));
EXTERNAL PROCEDURE CENT_B;
EXTERNAL PROCEDURE OPNY_D(REAL DIF,TT(2.0));
EXTERNAL PROCEDURE OPNY_A(REAL NEWH,TT(2.0));
EXTERNAL PROCEDURE CENT_Y;

PROCEDURE OPEN_B_DEL(REAL DIF,TT(2.0));
	OPNB_D(DIF,TT);

PROCEDURE OPEN_B_ABS(REAL NEWH,TT(2.0));
	OPNB_A(NEWH,TT);

PROCEDURE DRIVE_B_DEL(INTEGER J; REAL DIF,TT(2.0));
	DR_B_D(J, DIF,TT);

PROCEDURE DRIVE_B_ABS(INTEGER J; REAL NEWJ,TT(2.0));
	DR_B_A(J,NEWJ,TT);

COMMENT REQUIRE "
WILL REQUIRE FOLLOWING LOAD MODULES DURING LOADING
	TLKF5a[PNT,HE]
	TLKF3a[PNT,HE]
	MOVARM[PNT,HE]
	BEJCZY[PNT,HE]
	ARMSOL[PNT,HE]
" MESSAGE;